
--[[--------------------------------------------------------------------------

Inny's A-Star Pathfinding Demo

This is an A-Star Engine I came up with many years ago, and have decided to
bring into the future.

--]]--------------------------------------------------------------------------

------------------------------------------------------------------------------
-- The Library portion, steal this code, and the local
-- Implementation of A* Heuristic Path Finding
------------------------------------------------------------------------------
local insert = table.insert
local abs = math.abs
local remove = table.remove
local pairs = pairs
local floor = math.floor
local concat = table.concat
local function __nullFunction() end

local PathFinder = {}
local PathFinder_mt = { __index = PathFinder }

local TIMEOUT = 128
local NODECAP = 1024
local adjlist = {{0, -1, 'N'}, {0, 1, 'S'}, {-1, 0, 'W'}, {1, 0, 'E'}}

-- If you supply your own heuristic, it should match the function signature
-- of this ManhattanDistance function.
local function defaultManhattan( startx, starty, currx, curry, targx, targy )
  local dx1, dy1 = currx - targx, curry - targy
  local dx2, dy2 = startx - targx, starty - targy
  local cross = abs(dx1*dy2 - dx2*dy1)
  return (abs(targx-currx) + abs(targy-curry)) * (1+(cross*0.001))
end

local function lochash(x, y)
  return (y * 1000 + x)
end

-- Useful if you build your own builder function
function PathFinder.dehash( id )
  return floor(id%1000), floor(id/1000)
end

-- Your own builder will have this same signature:
-- * target: the ID of the tx, ty supplied to getPath
-- * parent: a lookup list of the parent ID of each node on the path
-- * dir: a lookup list of the NSWE direction to each node on the path
-- * closed: a list of closed nodes, useful for debugging
local function defaultBuilder( target, parent, dir, closed )
  local backpath = {}
  while target do
    insert(backpath, dir[target])
    target = parent[target]
  end
  local N = #backpath
  local m = floor(N/2)
  for i = 1, m do
    backpath[i], backpath[N-i+1] = backpath[N-i+1], backpath[i]
  end
  return concat(backpath)
end

-- ignore the man behind the curtain
local function sortPriorityQueue( q, f )
  table.sort(q, function( a, b )
    return (f[a] > f[b])
  end)
end

-- Call pathfinderobject:getPath to draw a path from (sx, sy) to (tx, ty)
-- Returns the result of the builder function supplied to PathFinder.new
function PathFinder:getPath( sx, sy, tx, ty )
  sx, sy = floor(sx), floor(sy)
  tx, ty = floor(tx), floor(ty)
  local dist = self.heuristic
  local blocked = self.blocked
  local startID, targetID = lochash(sx, sy), lochash(tx, ty)
  local startDist = dist(sx, sy, sx, sy, tx, ty)
  local cap = self.nodeCap
  local known, closed, f, g, h, x, y = {}, {}, {}, {}, {}, {}, {}
  local parent, dir = {}, {}
  local openqueue = {}
  local opennodes, nodecount, nodetimeout = 1, 0, self.coTimeout

  known[startID], x[startID], y[startID] = true, sx, sy
  g[startID], h[startID], f[startID] = 0, startDist, startDist
  openqueue[1] = startID

  while (opennodes > 0) and (nodecount < cap) do
    opennodes, nodecount = opennodes - 1, nodecount + 1
    local bestID = remove( openqueue )
    if bestID == targetID then
      return self.builder(bestID, parent, dir, closed)
    end
    local qchanges = false
    local wasClosed = closed[bestID]
    closed[bestID] = true
    if (not wasClosed) then
      local bestX, bestY = x[bestID], y[bestID]
      local nextG = g[bestID] + 1
      for _, v in ipairs( adjlist ) do
        local nx, ny, nd = bestX+v[1], bestY+v[2], v[3]
        local id = lochash(nx, ny)
        if not closed[id] then
          if not known[id] then
            if blocked( nx, ny ) then
              known[id], closed[id] = true, true
            else
              opennodes = opennodes + 1
              known[id] = true
              x[id], y[id] = nx, ny
              g[id], h[id] = nextG, dist(sx, sy, nx, ny, tx, ty)
              f[id] = g[id] + h[id]
              parent[id], dir[id] = bestID, nd
              insert(openqueue, id)
              qchanges = true
            end
          elseif nextG < g[id] then
            parent[id], dir[id] = bestID, nd
            g[id], f[id] = nextG, h[id] + nextG
            qchanges = true
          end
        end
      end
    end
    if qchanges then sortPriorityQueue( openqueue, f ) end
    if nodecount >= nodetimeout then
      nodetimeout = nodetimeout + TIMEOUT
      if coroutine.running() then coroutine.yield() end
    end
  end
  return nil, "Unable to find path (after "..nodecount.." nodes)"
end

-- Use Pathfinder.new to construct a pathfinder object
-- * the blocked function is required
-- * the builder function would be helpful if my "NSWE" directions aren't helpful
-- * nodecap can be raised if a 1024 node search is insufficient
-- * the heuristic function can be changed to something better than Manhattan
-- * timeout is how many nodes per coroutine cycle to consider.
function PathFinder.new( blocked, builder, nodecap, heuristic, timeout )
  local self = setmetatable( {}, PathFinder_mt )
  self.blocked = blocked or __nullFunction
  self.builder = builder or defaultBuilder
  self.nodeCap = nodecap or NODECAP
  self.heuristic = heuristic or defaultManhattan
  self.coTimeout = timeout or TIMEOUT
  return self
end

-- return PathFinder
-- Uncomment the previous line if you want to use this as a lua library
-- Use as local PathFinder = require "PathFinder"

------------------------------------------------------------------------------
-- The Demo portion, you can steal this also if you want.
------------------------------------------------------------------------------

local WHITE = { 255, 255, 255 }
local RED = { 192, 0, 0 }
local GREEN = { 0, 255, 0 }
local CYAN = { 0, 255, 255 }
local BLUE = { 0, 128, 255 }

local demoMap = {
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1},
}

local function demoToggleBlock( x, y )
  demoMap[y][x] = (demoMap[y][x]==0) and 1 or 0
end

local demoLeft = { 2, 2 }
local demoRight = { 19, 14 }

-- Example blocked function, performs lookups on the demoMap upvalue
-- You'll want to use a closure or something here.
local function demoBlocked( x, y )
  if (y < 1) or (y > #demoMap) or (x < 1) or (x > #demoMap[y]) then return false end
  return demoMap[y][x] > 0
end

local demoPath
local demoNodes
local demoNodeCount = 0

-- Exmaple builder, returns a list of X and Ys from start to finish.
local function demoBuilder( target, parent, dir, closed )
  local backpath = {}
  while target do
    local id = parent[target]
    if id then
      local x, y = PathFinder.dehash(id)
      insert( backpath, { x, y } )
    end
    target = id
  end
  local N = #backpath
  local m = floor(N/2)
  for i = 1, m do
    backpath[i], backpath[N-i+1] = backpath[N-i+1], backpath[i]
  end

  demoNodes = {}
  demoNodeCount = 0
  for id, _ in pairs(closed) do
    local x, y = PathFinder.dehash(id)
    insert(demoNodes, { x, y } )
    demoNodeCount = demoNodeCount + 1
  end

  return backpath
end

-- Here's an example of how to use the new and getPath functions
local demoFinder = PathFinder.new( demoBlocked, demoBuilder )
demoPath = demoFinder:getPath( demoLeft[1], demoLeft[2], demoRight[1], demoRight[2] )

-- And now, some Love specific code.

local function drawBlock( x, y, color )
  love.graphics.setColor( color )
  love.graphics.rectangle( "fill", (x-1)*16, (y-1)*16, 16, 16 )
end

function love.draw()
  if demoNodes then
    for _, v in pairs( demoNodes ) do
      drawBlock( v[1], v[2], RED )
    end
  end
  if demoPath then
    for _, v in pairs( demoPath ) do
      drawBlock( v[1], v[2], BLUE )
    end
  end
  for y = 1, #demoMap do
    for x = 1, #demoMap[y] do
      if demoMap[y][x] > 0 then
        drawBlock( x, y, WHITE )
      end
    end
  end
  drawBlock( demoLeft[1], demoLeft[2], GREEN )
  drawBlock( demoRight[1], demoRight[2], CYAN )
end

function love.mousepressed( x, y, button )
  if button == "m" then
    demoToggleBlock(1+floor(x/16), 1+floor(y/16))
  elseif (button == "l") or (button == "r") then
    local t = (button == "l") and demoLeft or demoRight
    t[1], t[2] = 1+floor(x/16), 1+floor(y/16)
  end
  local errMesg
  demoPath, errMesg = demoFinder:getPath( demoLeft[1], demoLeft[2], demoRight[1], demoRight[2] )
  if not demoPath then
    demoNodes, demoNodeCount = nil, nil
  end
  print(demoNodeCount, errMesg)
end

------------------------------------------------------------------------------
-- Thanks, Have a nice day.
------------------------------------------------------------------------------

